# 导入必要的库
import pyb       # 用于硬件控制（LED、UART等）
import time      # 时间相关操作
import image     # 图像处理库
import sensor    # 摄像头传感器控制
from pyb import UART  # 串口通信

# 全局配置参数
enable_lens_corr = False  # 是否启用镜头畸变校正
BINARY_VISIBLE = True     # 是否显示二值化图像

# 初始化摄像头传感器
sensor.reset()                      # 重置摄像头
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为RGB565（比灰度更快）
sensor.set_framesize(sensor.QVGA)   # 设置分辨率为QVGA（320x240）
sensor.skip_frames(time=2000)      # 等待设置生效（2秒）
sensor.set_auto_exposure(False)    # 关闭自动曝光（颜色识别需要稳定光照）
sensor.set_auto_whitebal(False)    # 关闭自动白平衡

# 定义颜色阈值（LAB色彩空间）
change_threshold = (0, 74, -128, 127, -128, -15)  # 变化检测阈值
green_threshold = (0, 100, -5, 127, -128, -18)    # 绿色阈值
green_threshold_2 = (55, 86, -66, -22, -26, 38)   # 辅助绿色阈值
black_threshold = (30, 0, 26, -20, 19, -16)       # 黑色阈值
red = [(22, 80, 35, 77, -50, 22)]    # 红色阈值列表
green = [(60, 88, -56, -29, -26, 7)] # 绿色阈值列表
blue = [(47, 71, -27, 4, -71, -31)]  # 蓝色阈值列表

# 传感器高级设置
sensor.set_auto_gain(0)   # 关闭自动增益（颜色跟踪必需）
sensor.set_auto_whitebal(0)  # 关闭白平衡（颜色跟踪必需）

# 初始化变量
min_degree = 0        # 直线检测最小角度
max_degree = 179      # 直线检测最大角度
uart = UART(3, 115200) # 初始化UART3，波特率115200
clock = time.clock()  # 创建时钟对象用于FPS计算

# LED指示灯初始化（三色全亮可能表示系统就绪）
pyb.LED(1).on()  # 红色LED
pyb.LED(2).on()  # 绿色LED
pyb.LED(3).on()  # 蓝色LED

# 数据存储数组（用于中值滤波）
Hx1 = [0]*15  # 存储检测到的线段起点x坐标
Hy1 = [0]*15  # 存储线段起点y坐标
# ...（其他类似数组省略）

# ============================================================================
# 中值滤波函数
# 输入：数据列表和长度
# 输出：中位数值
# ============================================================================
def median(data, length):
    sorted_data = sorted(data, reverse=True)
    if length % 2 == 1:
        return sorted_data[length//2]
    else:
        return (sorted_data[length//2-1] + sorted_data[length//2])/2

# ============================================================================
# 主循环
# ============================================================================
while(True):
    read_data = uart.read()  # 读取串口指令
    clock.tick()  # 更新时钟

    # 指令解析部分
    if read_data:
        read_str = read_data.decode('utf-8')
        print("Received Command:", read_str)

        # 模式切换逻辑
        if read_str == 'wcop':    # 颜色块识别模式
            op = 1
            sensor.set_auto_exposure(True)
        elif read_str == 'wczp':  # 底边校正模式
            op = 2
        # ...（其他模式切换条件省略）

    # ========================================================================
    # 各模式处理逻辑
    # ========================================================================
    if op == 1:  # 颜色块识别模式
        img = sensor.snapshot()
        # 红色块检测
        blob_red = img.find_blobs(red, area_threshold=10000, margin=10)
        if blob_red and not r_mod:
            # 发送红色块中心坐标：0X41 0X52 0XBB
            uart.writechar(0X41)
            uart.writechar(int(blob_red[0].cx()))
            uart.writechar(int(blob_red[0].cy()))
            uart.writechar(0XBB)
            print("Red block detected")

    elif op == 2:  # 边缘校正模式
        img = sensor.snapshot()
        # 进行线段检测和回归分析
        lines = img.find_line_segments(merge_distance=0, max_theta_diff=80)
        # 进行中值滤波后发送线段坐标
        # ...（具体处理逻辑省略）

    elif op == 3:  # 圆形放置位检测
        img.binary([green_threshold])  # 应用绿色阈值
        circles = img.find_circles(threshold=12000, r_min=45, r_max=50)
        if circles:
            # 发送圆心坐标：0X52 [x] [y] 0XBB
            uart.writechar(0X52)
            uart.writechar(int(circles[0].x()/2))  # 坐标缩放可能适应分辨率变化
            uart.writechar(int(circles[0].y()/2))
            uart.writechar(0XBB)

    elif op == 6:  # 二维码扫描模式
        img = sensor.snapshot()
        for code in img.find_qrcodes():
            # 发送二维码内容格式：QR:[content]\r\n
            uart.write(f"QR:{code.payload()}\r\n")

    # ========================================================================
    # 通用显示处理
    # ========================================================================
    if BINARY_VISIBLE:  # 如果需要显示处理后的图像
        img.binary([current_threshold])  # 显示当前阈值的二值化图像
        img.draw_string(0, 0, "FPS:%.2f" % clock.fps())  # 显示实时帧率

    print("FPS:", clock.fps())  # 在终端输出帧率信息
